#include <FastSerial.h>
#include <avr/eeprom.h>

#include <LCD_1602.h>

#include "ppm.h"
#include "define.h"

#include <iic.h>
#include <Matrix3.h>
#include <Vector3.h>

#include <ACCEL.h>
#include <AHRS_DCM.h>
#include <Compass.h>
#include <Gyro.h>


uint32_t system_ms;
int16_t servo_out[3];

AHRS_DCM ahrs;
LCD_1602 lcd;

uint8_t ver_eeprom[2] EEMEM;

FastSerialPort0(Serial);

void setup()
{
    Serial.begin(57600);             //设置COM波特率
    Serial.print("booting...");
    
    lcd.init();
    i2c_init();
    keyboard_init();

    PPM::init();
    ahrs.init();
    load_setting();
    ahrs.calibrate(IMU_GYRO);
    ahrs.reset(true, true);
//    ahrs.update();
//    ahrs.set_zero();

    menu_display_main();
    //key_enable = true;*/
    Serial.println("done");
}

void loop ()
{
    static uint32_t last_update_ms = 0;
    system_ms = millis();
    if (system_ms < last_update_ms)
        return;
    last_update_ms = system_ms + 10;  //100Hz;
    ahrs.update();
    control_update();
    ppm_update();
#if EULER_OUTPUT > 0
    euler_output();
#endif    
}


void ppm_update()
{
    for (uint8_t servo = 0; servo < 3; servo++)
        PPM::write_servo(servo, servo_out[servo]);

    if (!PPM::ppm_in)
    {
        for (uint8_t i = 0; i < 8; i++)
            PPM::write_channel_pwm(i, 1520);
    }
    else
        PPM::ppm_in = false;
}

void euler_output()
{
    static int8_t counter = 0;
    if (++counter > 5)
    {
        Serial.printf("!ANG:%f,%f,%f\n", ahrs.euler_angle[0]/10.0f, ahrs.euler_angle[1]/10.0f, ahrs.euler_angle[2]/10.0f);
        counter = 0;
    }
}

void load_setting()
{
    uint8_t ver[2];
    eeprom_read_block(ver, ver_eeprom, sizeof(ver));
    Serial.println("load setting");
    if (ver[0] != 'H' || ver[1] != VER)
    {
        ver[0] = 'H';
        ver[1] = VER;
        Serial.println("Init PPM setting");
        eeprom_write_block(ver, ver_eeprom, sizeof(ver));
        PPM::save_setting();  //write default setting
    }
    else
    {
        PPM::load_setting();
        ahrs.load_setting();
    }
}


